package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.spot.SensorServer;
import com.grt192.controller.spot.ServantedSensorService;
import com.grt192.core.CommandArray;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.spot.DemoSwitchListener;
import com.grt192.mechanism.spot.SLight;
import com.grt192.sensor.DemoJoystick;
import com.grt192.sensor.spot.GRTDemoSwitch;
import com.grt192.utils.Util;

/**
 * A macro which sends Accelerometer tilt data with the TelemetryServer class
 * 
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class SpotJoyStickMacro extends GRTObject implements Macro,
		SensorChangeListener, TelemetryPorts, DemoSwitchListener {

	private static final double MAX_G = 1.5;
	// lights
	private SLight[] lights = SLight.get();
	private static int[] accelLights = new int[] { 2, 3, 4 };
	private static int accelChangeLight = 7;
	static final double CHANGE_THRESHOLD = .1;
	// joystick
	private DemoJoystick joy;
	private GRTDemoSwitch sw0, sw1;
	// sensor service
	private SensorServer server;
	private ServantedSensorService joyServer, sw0Serve, sw1Serve;

	public SpotJoyStickMacro() {
		setPrinting(true);
	}

	/** Start this macro */
	public int execute() {
		initLights();
		startSensors();

		while (true) {
			Util.sleep(1000);
		}
	}

	/** Waiting status LEDs */
	public void initLights() {
		log("initializing lights...");
		for (int i = 0; i < accelLights.length; i++) {
			lights[accelLights[i]].rawColor(GRTDemoLED.Color.RED);
		}
		lights[accelChangeLight].rawColor(GRTDemoLED.Color.TURQUOISE);
		log("Lights initialization success!");
	}

	/** Initialize and start sensors */
	public void startSensors() {
		log("Initializing joystick...");
		joy = new DemoJoystick(25, "joy");
		joy.setChangeThreshold(CHANGE_THRESHOLD);
		joy.addSensorChangeListener(this);
		joy.start();
		log("Joystick initialization success!");

		log("Initializing switches...");
		sw0 = new GRTDemoSwitch(0, 250, "sw0");
		// sw1 = new GRTDemoSwitch(1, 250, "sw1");
		sw0.addSwitchListener(this);
		sw0.start();
		// sw1.start();

		log("Initializing Telemetry Server for Joystick");
		joyServer = new ServantedSensorService(joy, JOYSTICK_PORT);
		joyServer.addRemoteListeners(new int[] { 0, 80 });// basestation
		joyServer.start();
		log("Telemetry server initialization success!");

		log("Initializing Telemetry Server for Switches...");
		sw0Serve = new ServantedSensorService(sw0, DEMOSWITCH0_PORT);
		// sw1Serve = new ServentedSensorService(sw1, DEMOSWITCH1_PORT);
		// sw0Serve.setPrinting(true);
		// // sw1Serve.setPrinting(true);
		sw0Serve.addRemoteListeners(new int[] { 0, 80 });// basestation
		// sw1Serve.addRemoteListener(80);// basestation
		sw0Serve.start();
		// sw1Serve.start();
		// // server = new TelemetryServer(joy, JOYSTICK_PORT);
		// // server.start();
		log("Telemetry server initialization success!");

	}

	/**
	 * Converts a number in a known range [inMin... inMax] to a new range
	 * [outMin... outMax]
	 * 
	 * @param intMin
	 * @param intMax
	 * @param outMin
	 * @param outMax
	 * @param in
	 * @return
	 */
	private static double convertPercent(double intMin, double intMax,
			double outMin, double outMax, double in) {
		/*
		 * to get a percent: find out how the number, shifted to zero, compares
		 * to the range shifted to zero.
		 */
		double percent = (in - intMin) / (intMax - intMin);
		double newValue = (percent * (outMax - outMin)) + outMin;
		return newValue;

	}

	public void sensorStateChanged(SensorEvent e, String key) {
		int c = (int) convertPercent(-MAX_G, MAX_G, 0, 255, e.getData(key));
		CommandArray color = new CommandArray(255 - c, c, 0);
		if (key.equals(DemoJoystick.X)) {
			lights[accelLights[0]].rawColor(color);
			// log("X:\t" + e.getData(key));

		} else if (key.equals(DemoJoystick.Y)) {
			lights[accelLights[1]].rawColor(color);

		} else if (key.equals(DemoJoystick.Z)) {
			lights[accelLights[2]].rawColor(color);

		}

		lights[accelChangeLight].blinkToBlack();
	}

	public void switchPressed(GRTDemoSwitch source) {
		// TODO Auto-generated method stub
		log("Press");
	}

	public void switchReleased(GRTDemoSwitch source) {
		// TODO Auto-generated method stub
		log("Release");
	}
}
